/*
* @File Name iK.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\iK\iK.c
* @Author: Ruige_Lee
* @Date:   2019-04-24 19:11:38
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-05-19 15:40:42
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/


#include <stdint.h>

#include "xil_assert.h"

#include "iK/iK.h"
#include "math.h"

#define TOOLADDITION

// announce tool posture == end Coordinate
double ik_aimRotation[3][3] = {{-1.000,0.0000,0.0000},{0.000,-1.0000,0.0000},{0.000,0.0000,-1.0000}};
// double aim_toolPos[3] = {100,0,0};

float ik_aimConfig[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
// double ik_preConfig[6] = {0.0,0.0,0.0,0.0,0.0,0.0}





static void ik_solve(float* aim_toolPos)
{
	double theta1,theta2,theta3,theta4,theta5,theta6;
	double wristX,wristY,wristZ;
	double N1,N2,N3,O1,O2,O3,A1,A2,A3;

	N1 = ik_aimRotation[0][0];
	N2 = ik_aimRotation[1][0];
	N3 = ik_aimRotation[2][0];
	O1 = ik_aimRotation[0][1];
	O2 = ik_aimRotation[1][1];
	O3 = ik_aimRotation[2][1];
	A1 = ik_aimRotation[0][2];
	A2 = ik_aimRotation[1][2];
	A3 = ik_aimRotation[2][2];

	wristX = aim_toolPos[0] + ik_aimRotation[0][2] * 40;
	wristY = aim_toolPos[1] + ik_aimRotation[1][2] * 40;
	wristZ = aim_toolPos[2] + ik_aimRotation[2][2] * 40;



	if ( wristX > 0.00001 || wristX < -0.000001 )
	{
		;
	}
	else
	{
		wristX = 0.001;
	}
	theta1 = atan2(wristY,wristX);

	double s3 = ( (wristX*wristX) / ( cos(theta1)*cos(theta1) ) + (wristZ-264)*(wristZ-264) - 97844.29) / 97785;
	theta3 = asin (s3);
	
	double d = 217.3 * cos( theta3 );
	double f = 217.3 * sin(theta3) + 225;
	double g = wristX / cos(theta1);
	double h = wristZ - 264;

	theta2 = atan2 (( h * d - g * f) , ( g * d + h * f )); 





	double r11 = N3*sin(theta2 + theta3) + N2*sin(theta1)*cos(theta2 + theta3) + cos(theta1)*N1*cos(theta2 + theta3); 
	double r21 = N2*cos(theta1) - N1*sin(theta1);
	double r31 = N3*cos(theta2 + theta3) - N2*sin(theta1)*sin(theta2 + theta3) - cos(theta1)*N1*sin(theta2 + theta3);
	
	double r12 = O3*sin(theta2 + theta3) + O2*sin(theta1)*cos(theta2 + theta3) + cos(theta1)*O1*cos(theta2 + theta3);
	double r22 = O2*cos(theta1) - O1*sin(theta1);
	double r32 = O3*cos(theta2 + theta3) - O2*sin(theta1)*sin(theta2 + theta3) - cos(theta1)*O1*sin(theta2 + theta3);
	
	double r13 = A3*sin(theta2 + theta3) + A2*sin(theta1)*cos(theta2 + theta3) + cos(theta1)*A1*cos(theta2 + theta3);
	double r23 = A2*cos(theta1) - A1*sin(theta1);
	double r33 = A3*cos(theta2 + theta3) - A2*sin(theta1)*sin(theta2 + theta3) - cos(theta1)*A1*sin(theta2 + theta3); 

	
	//theta5 = atan2( r13 , -sqrt(r11*r11 + r12*r12));

	theta4 = atan2( ( -r23 ),( r33 ));
	
	theta6 = atan2( ( r12 ),( r11 ));
	
	if ( abs(theta4 + 3.14159) < abs(theta4) )
	{

		theta4 = theta4 + 3.14159;
		theta5 = atan2( r13 , sqrt(r11*r11 + r12*r12) );
		theta6 = theta6 - 3.14159;
	}
	else
	{
		theta5 = atan2( r13 , -sqrt(r11*r11 + r12*r12) );
	}


	ik_aimConfig[0] = (float)(theta1);
	ik_aimConfig[1] = (float)(theta2);
	ik_aimConfig[2] = (float)(theta3);
	ik_aimConfig[3] = (float)(theta4);
	ik_aimConfig[4] = (float)(theta5);
	ik_aimConfig[5] = (float)(theta6);
}



// checkin fifo of ik,
// parameter start pose and end pose
int32_t ik_checkIn(uint8_t robot, float *startPose, float *endPose, uint16_t step)
{
	// assert( step != 0 );
	// assert( step <= 65534 );

	float stepPose[3];
	float radAngle[6];
	float aim_toolPos[3];

	int16_t robotConfig[6];


	for ( uint8_t axis = 0; axis < 6; axis ++ )
	{
		radAngle[axis] = stepper_getRad( robot, axis );
	}

	stepPose[0] = (endPose[0] - startPose[0]) / step;
	stepPose[1] = (endPose[1] - startPose[1]) / step;
	stepPose[2] = (endPose[2] - startPose[2]) / step;

	aim_toolPos[0] = startPose[0];
	aim_toolPos[1] = startPose[1];
	aim_toolPos[2] = startPose[2];

	for ( uint16_t i = 0; i < step+2; i++ )
	{
		ik_solve(aim_toolPos);

		for ( uint8_t axis = 0; axis < 6; axis++ )
		{
			robotConfig[axis] = stepper_rad2Pluse( axis, (ik_aimConfig[axis] - radAngle[axis]));	
			radAngle[axis] = ik_aimConfig[axis];

		}
		while( 0 != stepper_fifoPush( robot, robotConfig ) )
		{
			;
		}

		aim_toolPos[0] += stepPose[0];
		aim_toolPos[1] += stepPose[1];
		aim_toolPos[2] += stepPose[2];
	}
	return 0;
}


void ik_fkCheck(uint8_t robot,float *FK)
{
	float angle[4];

	for ( uint8_t axis = 0; axis < 5; axis ++ )
	{
		angle[axis] = stepper_getRad( robot, axis );
	}

	float theta1 = angle[0];
	float theta2 = angle[1];
	float theta3 = angle[2];
	float theta4 = angle[3];
	float theta5 = angle[4];



	FK[0] = (float)((217.3*cos(theta1)*cos(theta2)*cos(theta3)) - 40*sin(theta5)*(cos(theta1)*cos(theta2)*cos(theta3) - cos(theta1)*sin(theta2)*sin(theta3)) - 40*cos(theta5)*(sin(theta1)*sin(theta4) - cos(theta4)*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2))) - 225*cos(theta1)*sin(theta2) - (217.3*cos(theta1)*sin(theta2)*sin(theta3)));
	FK[1] = (float)(40*cos(theta5)*(cos(theta1)*sin(theta4) + cos(theta4)*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2))) - 40*sin(theta5)*(cos(theta2)*cos(theta3)*sin(theta1) - sin(theta1)*sin(theta2)*sin(theta3)) - 225*sin(theta1)*sin(theta2) + (217.3*cos(theta2)*cos(theta3)*sin(theta1)) - (217.3*sin(theta1)*sin(theta2)*sin(theta3)));
	FK[2] = (float)(225*cos(theta2) + (217.3*cos(theta2)*sin(theta3)) + (217.3*cos(theta3)*sin(theta2)) - 40*sin(theta5)*(cos(theta2)*sin(theta3) + cos(theta3)*sin(theta2)) - 40*cos(theta4)*cos(theta5)*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + 264);

	return;
}

